#include "RotateExpMap.h"
#include <string.h>

RotateExpMap::RotateExpMap(Dof *x, Dof *y, Dof *z){
	mDofs.clear();
	mDofs.resize(3);
	mDofs[0]=x;
	mDofs[1]=y;
	mDofs[2]=z;
	mDofs[0]->setTrans(this);
	mDofs[1]->setTrans(this);
	mDofs[2]->setTrans(this);
	strcpy(mDofs[0]->getName(), "_expX");
	strcpy(mDofs[1]->getName(), "_expY");
	strcpy(mDofs[2]->getName(), "_expZ");
	mType = Transformation::T_ROTATEEXPMAP;
}

RotateExpMap::RotateExpMap(Dof *x, Dof *y, Dof *z, char *_name){
	mDofs.clear();
	mDofs.resize(3);
	mDofs[0]=x;
	mDofs[1]=y;
	mDofs[2]=z;
	mDofs[0]->setTrans(this);
	mDofs[1]->setTrans(this);
	mDofs[2]->setTrans(this);
	strcpy(mDofs[0]->getName(), "_expX");
	strcpy(mDofs[1]->getName(), "_expY");
	strcpy(mDofs[2]->getName(), "_expZ");
	mType = Transformation::T_ROTATEEXPMAP;
	strcpy(mName, _name);
}



Mat4d *RotateExpMap::getTransformMat(){
	Vec3d v(mDofs[0]->getValue(), mDofs[1]->getValue(), mDofs[2]->getValue());
	
	//Vec3d v(0.001, 0, 2 * M_PI);
	//Vec3d v2(0.001, 0, 0);

	//double theta2 = len(v2);
	//Vec3d vhat2 = vl_0;
	//vhat2= v2/theta2;
	//Quaternion q2(vhat2, theta2);
	//Mat3d test2 = q2.getMatrix();

	double theta=len(v);
	Vec3d vhat = vl_0;
	if(!isZero(theta)) vhat= v/theta;

	Mat3d mat = MakeSkewSymmetric(vhat);
	Mat3d rot;
	rot.MakeDiag();
	rot += sin(theta) * mat + (1 - cos(theta)) * mat * mat;

	Quaternion q(vhat, theta);
	
	rot = q.getMatrix();

	(*mTransform)=vl_1;
	for(int i=0; i<3; i++){
		for(int j=0; j<3; j++){
            (*mTransform)[i][j] = rot[i][j];
		}
	}
	return mTransform;
}

double RotateExpMap::get_dq_dv(int i, int j, double theta, Vec3d v, Vec3d vhat){
	double dq_dv=0;
	double sinc_theta_half = (sin(0.5*theta)/theta);
	if (i==0) dq_dv = -0.5*v[j]*sinc_theta_half;
	else {
		i=i-1;
		dq_dv = sinc_theta_half*moremath::delta(i,j) + 0.5*vhat[i]*vhat[j]*(cos(0.5*theta)-2*sinc_theta_half);
	}
	return dq_dv;
}

double RotateExpMap::get_dq_dv_approx(int i, int j, double theta, Vec3d v){
#if _DEBUG
	//printf("========== inside approx ==========\n");
#endif
	double dq_dv=0;
	double sinc_theta_half = Tsinc(theta);
	if (i==0) dq_dv = -0.5*v[j]*sinc_theta_half;
	else {
		i=i-1;
		dq_dv = sinc_theta_half*moremath::delta(i,j) + (v[i]*v[j]*(sqr(theta)/40-1))/24;
	}
	return dq_dv;
}

Mat3d RotateExpMap::getDerivativeMatNew(Dof *d)
{
	Vec3d v(mDofs[0]->getValue(), mDofs[1]->getValue(), mDofs[2]->getValue());
	double theta=len(v);

	Vec3d vhat = vl_0;
	if(!isZero(theta)) vhat= v/theta;


	Mat3d omegaSkewSymmetric = MakeSkewSymmetric(vhat);
	Mat3d omegaSkewSymmetricDeriv;
	if (d == mDofs[0])
	{
		omegaSkewSymmetricDeriv = MakeSkewSymmetric(Vec3d(1, 0, 0));
	}
	else if (d == mDofs[1])
	{
		omegaSkewSymmetricDeriv = MakeSkewSymmetric(Vec3d(0, 1, 0));
	}
	else if (d == mDofs[2])
	{
		omegaSkewSymmetricDeriv = MakeSkewSymmetric(Vec3d(0, 0, 1));
	}
	else
	{
		assert(0);
	}
	omegaSkewSymmetricDeriv /= theta;
	omegaSkewSymmetricDeriv += -d->getValue() / (theta * theta) * omegaSkewSymmetric;
	Mat3d ret = vl_0;
	ret += cos(theta) * d->getValue() / theta * omegaSkewSymmetric;
	ret += sin(theta) * omegaSkewSymmetricDeriv;
	ret += sin(theta) * d->getValue() / theta * omegaSkewSymmetric * omegaSkewSymmetric;
	ret += (1 - cos(theta)) * (omegaSkewSymmetricDeriv * omegaSkewSymmetric + omegaSkewSymmetric * omegaSkewSymmetricDeriv);
	return ret;
}

Mat4d *RotateExpMap::getDerivativeMat(Dof *d){


	Vec3d v(mDofs[0]->getValue(), mDofs[1]->getValue(), mDofs[2]->getValue());
	double theta=len(v);


	Vec3d vhat = vl_0;
	if(!isZero(theta)) vhat= v/theta;

	//Vec3d newTest = (theta - 2 * M_PI * i) * vhat;
	//mDofs[0]->setValue(newTest[0]);
	//mDofs[1]->setValue(newTest[1]);
	//mDofs[2]->setValue(newTest[2]);

	//v = Vec3d(mDofs[0]->getValue(), mDofs[1]->getValue(), mDofs[2]->getValue());
	//theta=len(v);


	//vhat = vl_0;
	//if(!isZero(theta)) vhat= v/theta;

	Quaternion q(vhat, theta);
	
	// compute derivative of R wrt each qi
	vector<Mat3d> dR_dq;
	dR_dq.resize(4);
	for(int i=0; i<4; i++) dR_dq[i] = q.getDerivativeMatrix(i);

	// derivative wrt which dof 
	int j=-1;
	for(int i=0; i<mDofs.size(); i++) if(d->getID()==mDofs[i]->getID()) j=i;
	assert(j!=-1);

	// compute derivative of qi's wrt v[j]
	vector<double> dq_dv;
	dq_dv.resize(4);



	if(fabs(theta)<EPSILON) {
		for(int i=0; i<4; i++){
			dq_dv[i] = get_dq_dv_approx(i, j, theta, v);
		}
	}
	else {
		for(int i=0; i<4; i++){
			dq_dv[i] = get_dq_dv(i, j, theta, v, vhat);
		}
	}

	// compute the reqd derivative
	(*mDeriv)=vl_0;
	Mat3d mat=vl_0;

	for(int i=0; i<4; i++){
        mat+=dR_dq[i]*dq_dv[i];
	}
	
	//Mat3d testMat = getDerivativeMatNew(d);
	//Mat4d* R = getTransformMat();
	//cout << "R:       " << *R << endl;
	//cout << "Mat:     " << mat << endl;
	//cout << "TestMat: " << testMat << endl;

	for(int i=0; i<3; i++){
		for(int j=0; j<3; j++){
            (*mDeriv)[i][j] = mat[i][j];
		}
	}


	return mDeriv;
}

double RotateExpMap::get_dq_dv_dv(int i, int j, int k, double theta, Vec3d vhat){
	double dq_dv_dv=0;
	
	double cos_half = cos(0.5*theta);
	double sin_half = sin(0.5*theta);
	double sinc_theta_half = (sin_half/theta);

	if (i==0) {
		dq_dv_dv = -0.5*sinc_theta_half*moremath::delta(j,k);
		dq_dv_dv += - 0.25*vhat[j]*vhat[k]*(cos_half-2*sinc_theta_half);
	}
	else {
		i=i-1;
		dq_dv_dv = ((cos_half-2*sinc_theta_half)/(2*theta))*(vhat[k]*moremath::delta(i,j)+vhat[i]*moremath::delta(k,j)+vhat[j]*moremath::delta(i,k));
		dq_dv_dv += 0.25*vhat[i]*vhat[j]*vhat[k]*(-sin_half-(6/theta)*(cos_half-2*sinc_theta_half));
	}
	return dq_dv_dv;
}

double RotateExpMap::get_dq_dv_dv_approx(int i, int j, int k, double theta, Vec3d v){
#if _DEBUG
	//printf("========= inside approx ==========\n");
#endif
	double dq_dv_dv=0;

	if (i==0) {
		dq_dv_dv = v[j]*v[k]/48 - 0.5*Tsinc(theta)*moremath::delta(j,k);
	}
	else {
		i=i-1;
		dq_dv_dv = -(v[k]*moremath::delta(i,j)+v[i]*moremath::delta(k,j)+v[j]*moremath::delta(i,k))/24;
		dq_dv_dv += (sqr(theta)/960)*(v[i]*moremath::delta(k,j)+v[j]*moremath::delta(i,k));
		dq_dv_dv += v[i]*v[j]*v[k]/480;
	}
	return dq_dv_dv;
}

Mat4d *RotateExpMap::getDerivativeMat(Dof *d1, Dof *d2){
	Vec3d v(mDofs[0]->getValue(), mDofs[1]->getValue(), mDofs[2]->getValue());
	double theta=len(v);

	Vec3d vhat = vl_0;
	if(!isZero(theta)) vhat= v/theta;
	Quaternion q(vhat, theta);
	
	// derivative wrt which mDofs
	int j=-1, k=-1;
	for(int i=0; i<mDofs.size(); i++) {
		if(d1->getID()==mDofs[i]->getID()) j=i;
		if(d2->getID()==mDofs[i]->getID()) k=i;
	}
	assert(j!=-1);
	assert(k!=-1);

	// compute derivative of R wrt each qi
	vector<Mat3d> dR_dq;
	dR_dq.resize(4);
	for(int i=0; i<4; i++) dR_dq[i] = q.getDerivativeMatrix(i);

	// compute derivative of R wrt each qi and ql
	vector<vector<Mat3d> > dR_dq_dq;
	dR_dq_dq.resize(4);
	for(int i=0; i<4; i++) dR_dq_dq[i].resize(4);
	for(int i=0; i<4; i++) {
		for(int l=i; l<4; l++) {
			dR_dq_dq[i][l] = q.getDerivativeMatrix(i, l);
			dR_dq_dq[l][i]=dR_dq_dq[i][l];
		}
	}

	// compute derivative of qi's wrt v[j]
	vector<double> dq_dvj;
	dq_dvj.resize(4);
	if(fabs(theta)<EPSILON) {
		for(int i=0; i<4; i++){
			dq_dvj[i] = get_dq_dv_approx(i, j, theta, v);
		}
	}
	else {
		for(int i=0; i<4; i++){
			dq_dvj[i] = get_dq_dv(i, j, theta, v, vhat);
		}
	}

	// compute derivative of qi's wrt v[k]
	vector<double> dq_dvk;
	dq_dvk.resize(4);
	if(fabs(theta)<EPSILON) {
		for(int i=0; i<4; i++){
			dq_dvk[i] = get_dq_dv_approx(i, k, theta, v);
		}
	}
	else {
		for(int i=0; i<4; i++){
			dq_dvk[i] = get_dq_dv(i, k, theta, v, vhat);
		}
	}

	// compute double derivative of qi's wrt v[j] and v[k]
	vector<double> dq_dvj_dvk;
	dq_dvj_dvk.resize(4);
	if(fabs(theta)<EPSILON) {
		for(int i=0; i<4; i++){
			dq_dvj_dvk[i] = get_dq_dv_dv_approx(i, j, k, theta, v);
		}
	}
	else {
		for(int i=0; i<4; i++){
			dq_dvj_dvk[i] = get_dq_dv_dv(i, j, k, theta, vhat);
		}
	}

	// compute the reqd derivative
	(*mDeriv)=vl_0;
	Mat3d mat=vl_0;

	for(int i=0; i<4; i++){
        mat+=dR_dq[i]*dq_dvj_dvk[i];
		Mat3d localmat=vl_0;
		for(int l=0; l<4; l++){
			localmat+=dR_dq_dq[i][l]*dq_dvk[l];
		}
		mat+=localmat*dq_dvj[i];
	}
	
	for(int i=0; i<3; i++){
		for(int j=0; j<3; j++){
            (*mDeriv)[i][j] = mat[i][j];
		}
	}

	return mDeriv;
}

void RotateExpMap::applyGLTransform(){
	Vec3d v(mDofs[0]->getValue(), mDofs[1]->getValue(), mDofs[2]->getValue());
	double theta=len(v);
	Vec3d vhat = vl_0;
	if(!isZero(theta)) {
		vhat= v/theta;
		glRotatef(theta*180/M_PI, vhat[0], vhat[1], vhat[2]);
	}
}

bool RotateExpMap::AdjustDofs()
{
	Vec3d v(mDofs[0]->getValue(), mDofs[1]->getValue(), mDofs[2]->getValue());

	double theta=len(v);

	Vec3d axis = v / theta;
	if (theta > M_PI)
	{
		mDofs[0]->setValue(axis[0] * (theta - 2 * M_PI));
		mDofs[1]->setValue(axis[1] * (theta - 2 * M_PI));
		mDofs[2]->setValue(axis[2] * (theta - 2 * M_PI));
		//Vec3d test(mDofs[0]->getValue(), mDofs[1]->getValue(), mDofs[2]->getValue());
		//double newTheta = len(test);
		//Vec3d testAxis = test/ newTheta;
		return true;
	}
	return false;
}

bool RotateExpMap::NeedDofAdjustment()
{
	Vec3d v(mDofs[0]->getValue(), mDofs[1]->getValue(), mDofs[2]->getValue());

	double theta=len(v);

	if (theta > M_PI)
		return true;
	else
		return false;
}



Mat3d MakeSkewSymmetric(const Vec3d &v)
{
	Mat3d result;
	result.MakeZero();
	result[0][1] = -v[2];
	result[0][2] = v[1];
	result[1][0] = v[2];
	result[1][2] = -v[0];
	result[2][0] = -v[1];
	result[2][1] = v[0];

	return result;
}

Vec3d FromSkewSymmetric(const Mat3d& m)
{
	Mat3d testM = m;
	if (abs(testM[0][0]) > EPSILON || abs(testM[1][1]) > EPSILON || abs(testM[2][2]) > EPSILON)
	{
		printf("Not skew symmetric matrix");
		//*((int*)0) = 0;
	}
	Vec3d ret;
	Vec3d unitY(0, 1, 0);
	Vec3d xz = m * unitY;
	ret[0] = xz[2];
	ret[2] = -xz[0];
	Vec3d unitZ(0, 0, 1);
	Vec3d y = m * unitZ;
	ret[1] = y[0];
	return ret;
}
